﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Media;
using System.Windows.Shapes;
using Microsoft.Research.Kinect.Nui;
using KinectNui = Microsoft.Research.Kinect.Nui;
using System.ComponentModel;
using System.Windows.Media.Imaging;
using System.Collections;
using Vector3 = Microsoft.Xna.Framework.Vector3;
using Matrix = Microsoft.Xna.Framework.Matrix;
using Vector = Microsoft.Research.Kinect.Nui.Vector;


namespace Microsoft.Samples.Kinect.WpfViewers
{
    /// <summary>
    /// Interaction logic for KinectDiagnosticViewer.xaml
    /// </summary>
    public partial class KinectDiagnosticViewer : UserControl
    {
        #region Public API
        public KinectDiagnosticViewer()
        {
            points = new List<Point>();
            LeftUpPoint = NullPoint;
            LeftDownPoint = NullPoint;
            RightUpPoint = NullPoint;
            RightDownPoint = NullPoint;
            InitializeComponent();
        }

        public RuntimeOptions MyRuntimeOptions { get; private set; }

        public void ReInitRuntime()
        {
            // Will call Uninitialize followed by Initialize.
            this.Kinect = this.Kinect;
        }

        public KinectNui.Runtime Kinect
        {
            get { return _Kinect; }
            set
            {
                //Clean up existing runtime if we are being set to null, or a new Runtime.
                if (_Kinect != null)
                {
                    _Kinect.DepthFrameReady -= new EventHandler<ImageFrameReadyEventArgs>(DepthImageReady);
                    _Kinect.VideoFrameReady -= new EventHandler<ImageFrameReadyEventArgs>(ColorImageReady);
                    _Kinect.Uninitialize();
                }

                _Kinect = value;

                if (_Kinect != null && _Kinect.Status == KinectStatus.Connected)
                {
                    // NOTE:  Skeletal tracking only works on one Kinect per process right now.
                    MyRuntimeOptions = RuntimeOptions.UseDepthAndPlayerIndex | RuntimeOptions.UseSkeletalTracking | RuntimeOptions.UseColor;
                    _Kinect.Initialize(MyRuntimeOptions);
                    if (MyRuntimeOptions.HasFlag(RuntimeOptions.UseSkeletalTracking))
                        _Kinect.SkeletonEngine.TransformSmooth = true;

                    _Kinect.VideoStream.Open(ImageStreamType.Video, 2, ImageResolution.Resolution640x480, ImageType.Color);
                    _Kinect.DepthStream.Open(ImageStreamType.Depth, 2, ImageResolution.Resolution320x240, ImageType.DepthAndPlayerIndex);

                    _Kinect.VideoFrameReady += new EventHandler<ImageFrameReadyEventArgs>(ColorImageReady);

                    lastTime = DateTime.MaxValue;
                    totalFrames = 0;
                    lastFrames = 0;

                    cam = _Kinect.NuiCamera;
                    this.sliderAngle.Value = cam.ElevationAngle;

                    _Kinect.DepthFrameReady += new EventHandler<ImageFrameReadyEventArgs>(DepthImageReady);
                
                }
            }
        }

        #endregion Public API

        #region Kinect ColorImage processing
        void ColorImageReady(object sender, ImageFrameReadyEventArgs e)
        {
            PlanarImage planarImage = e.ImageFrame.Image;
            byte[] convertedColorBits = planarImage.Bits;

            int width = 640, height = 480;
            int x = (int)Math.Round(ColorXPersentage * width);
            int y = (int)Math.Round(ColorYPersentage * height);
            FuckPoint(convertedColorBits, width, height, 10, x, y, 255, 0, 0);

            //An interopBitmap is a WPF construct that enables resetting the Bits of the image.
            //This is more efficient than doing a BitmapSource.Create call every frame.
            if (colorImageHelper == null)
            {
                colorImageHelper = new InteropBitmapHelper(planarImage.Width, planarImage.Height, convertedColorBits);
                kinectColorImage.Source = colorImageHelper.InteropBitmap;
            }
            else
            {
                colorImageHelper.UpdateBits(planarImage.Bits);
            }
        }

        private InteropBitmapHelper colorImageHelper = null;
        #endregion Kinect ColorImage processing

        public int FrameRate
        {
            get { return _FrameRate; }
            set
            {
                if (_FrameRate != value)
                {
                    _FrameRate = value;
                    frameRate.Text = _FrameRate.ToString();
                }
            }
        }

        #region DepthImage Processing
        private void DepthImageReady(object sender, ImageFrameReadyEventArgs e)
        {
            PlanarImage planarImage = e.ImageFrame.Image;
            depthBits = planarImage.Bits;
            convertedDepthBits = convertDepthFrame(depthBits, realDepth);

            int width = 320, height = 240;

            foreach (Point p in points)
            {
                int x = (int)Math.Round(p.X * width);
                int y = (int)Math.Round(p.Y * height);
                FuckPoint(convertedDepthBits, width, height, 5, x, y, 255, 0, 0);
            }
            do {
                int x = (int)Math.Round(LeftUpPoint.X * width);
                int y = (int)Math.Round(LeftUpPoint.Y * height);
                FuckPoint(convertedDepthBits, width, height, 5, x, y, 0, 0, 255);
            } while (false);

            do {
                int x = (int)Math.Round(RightUpPoint.X * width);
                int y = (int)Math.Round(RightUpPoint.Y * height);
                FuckPoint(convertedDepthBits, width, height, 5, x, y, 0, 0, 255);
            } while (false);

            do {
                int x = (int)Math.Round(LeftDownPoint.X * width);
                int y = (int)Math.Round(LeftDownPoint.Y * height);
                FuckPoint(convertedDepthBits, width, height, 5, x, y, 0, 0, 255);
            } while (false);

            do {
                int x = (int)Math.Round(RightDownPoint.X * width);
                int y = (int)Math.Round(RightDownPoint.Y * height);
                FuckPoint(convertedDepthBits, width, height, 5, x, y, 0, 0, 255);
            } while (false);


            //An interopBitmap is a WPF construct that enables resetting the Bits of the image.
            //This is more efficient than doing a BitmapSource.Create call every frame.
            if (depthImageHelper == null)
            {
                depthImageHelper = new InteropBitmapHelper(planarImage.Width, planarImage.Height, convertedDepthBits);
                kinectDepthImage.Source = depthImageHelper.InteropBitmap;
            }
            else
            {
                depthImageHelper.UpdateBits(convertedDepthBits);
            }

            calculateFrameRate();
        }

        private void calculateFrameRate()
        {
            ++totalFrames;

            DateTime cur = DateTime.Now;
            if (lastTime == DateTime.MaxValue || cur.Subtract(lastTime) > TimeSpan.FromSeconds(1))
            {
                FrameRate = totalFrames - lastFrames;
                lastFrames = totalFrames;
                lastTime = cur;
            }
        }

        // Converts a 16-bit grayscale depth frame which includes player indexes into a 32-bit frame
        // that displays different players in different colors
        byte[] convertDepthFrame(byte[] depthFrame16, int []realDepthFrame)
        {
            bool hasPlayerData = MyRuntimeOptions.HasFlag(RuntimeOptions.UseDepthAndPlayerIndex);

            int mindepth = -1, maxdepth = -1;
            for (int i16 = 0; i16 < depthFrame16.Length; i16 += 2)
            {
                int player = hasPlayerData ? depthFrame16[i16] & 0x07 : -1;
                int realDepth = 0;

                if (hasPlayerData)
                    realDepth = (depthFrame16[i16 + 1] << 5) | (depthFrame16[i16] >> 3);
                else
                    realDepth = (depthFrame16[i16 + 1] << 8) | (depthFrame16[i16]);

                if (realDepth == 0) continue;

                if (mindepth == -1 || realDepth < mindepth) mindepth = realDepth;
                if (maxdepth == -1 || realDepth > maxdepth) maxdepth = realDepth;
            }

//            labelAngle.Content = mindepth;
            
            for (int i16 = 0, i32 = 0, i8 = 0; i16 < depthFrame16.Length && i32 < depthFrame32.Length; i16 += 2, i32 += 4, i8++)
            {
                int realDepth = 0;

                if (hasPlayerData)
                    realDepth = (depthFrame16[i16 + 1] << 5) | (depthFrame16[i16] >> 3);
                else
                    realDepth = (depthFrame16[i16 + 1] << 8) | (depthFrame16[i16]);

                // transform 13-bit depth information into an 8-bit intensity appropriate
                // for display (we disregard information in most significant bit)

                //byte intensity = (byte)(255 - (255 * realDepth / 0x0fff));
                byte intensity = (byte)(255 * (realDepth - mindepth) / (double)(maxdepth - mindepth));

                realDepthFrame[i8] = realDepth;
                depthFrame32[i32 + RedIndex] = (byte)(intensity);
                depthFrame32[i32 + GreenIndex] = (byte)(intensity);
                depthFrame32[i32 + BlueIndex] = (byte)(intensity);
            }
            return depthFrame32;
        }
        #endregion DepthImage Processing

        #region Private State
        // We want to control how depth data gets converted into false-color data
        // for more intuitive visualization, so we keep 32-bit color frame buffer versions of
        // these, to be updated whenever we receive and process a 16-bit frame.
        private KinectNui.Runtime _Kinect;
        private Camera cam = null;
        private const int RedIndex = 2;
        private const int GreenIndex = 1;
        private const int BlueIndex = 0;
        private int _FrameRate = -1;
        private int totalFrames;
        private int lastFrames;
        private DateTime lastTime = DateTime.MaxValue;
        private List<Point> points;
        byte[] convertedDepthBits;
        byte[] depthBits;
        private byte[] depthFrame32 = new byte[640 * 480 * 4];
        private int[] realDepth = new int[640 * 480 * 4];
        private double ColorXPersentage = -1, ColorYPersentage = -1;
        private double DepthXPersentage = -1, DepthYPersentage = -1;
        private Point LeftUpPoint, LeftDownPoint, RightUpPoint, RightDownPoint, NullPoint = new Point(-1, -1);
        private InteropBitmapHelper depthImageHelper = null;
        private const int depthWidth = 320, depthHeight = 240;
        private const int colorWidth = 640, colorHeight = 480;
        private const int depthFrameWidth = 640, depthFrameHeight = 480;
        private const int colorFrameWidth = 640, colorFrameHeight = 480;
        private const double depthWidthf = depthWidth, depthHeightf = depthHeight;
        private const double colorWidthf = colorWidth, colorHeightf = colorHeight;
        private const double depthFrameWidthf = depthFrameWidth, depthFrameHeightf = depthFrameHeight;
        private const double colorFrameWidthf = colorFrameWidth, colorFrameHeightf = colorFrameHeight;
        private GFileWriter gFileWriter = new GFileWriter();
        #endregion Private State

        private void kinectDepthImage_MouseLeftButtonDown(object sender, System.Windows.Input.MouseButtonEventArgs e)
        {
            System.Windows.Point Position;
            Position = e.GetPosition(kinectDepthImage);
            DepthXPersentage = Position.X / depthFrameWidthf;
            DepthYPersentage = Position.Y / depthFrameHeightf;

            PointDepthPersentage.Text = string.Format("{0:0.00}% x {1:0.00}%", DepthXPersentage * 100, DepthYPersentage * 100);

            points.Add(new Point(DepthXPersentage, DepthYPersentage));
            LeftUpPoint = LeftDownPoint = RightUpPoint = RightDownPoint = points[0];
            foreach (Point pt in points)
            {
                if (pt.X + pt.Y < LeftUpPoint.X + LeftUpPoint.Y)
                    LeftUpPoint = pt;
                if (pt.X + pt.Y > RightDownPoint.X + RightDownPoint.Y)
                    RightDownPoint = pt;
                if (pt.X - pt.Y > RightUpPoint.X - RightUpPoint.Y)
                    RightUpPoint = pt;
                if (pt.X - pt.Y < LeftDownPoint.X - LeftDownPoint.Y)
                    LeftDownPoint = pt;
            }
            Point[] criticalPoints = { LeftUpPoint, RightUpPoint, LeftDownPoint, RightDownPoint };
            for (int k = 0; k < criticalPoints.Length; k++)
            {
                double avgX = 0, avgY = 0;
                int cnt = 0;
                foreach (Point pt in points)
                {
                    if (GMath.hypot(pt.X - criticalPoints[k].X, pt.Y - criticalPoints[k].Y) < 0.05)
                    {
                        avgX += pt.X;
                        avgY += pt.Y;
                        ++cnt;
                    }
                }
                avgX /= cnt;
                avgY /= cnt;
                criticalPoints[k] = new Point(avgX, avgY);
            }
            LeftUpPoint = criticalPoints[0];
            RightUpPoint = criticalPoints[1];
            LeftDownPoint = criticalPoints[2];
            RightDownPoint = criticalPoints[3];

            labelTmp.Content = points.Count.ToString();
        }

        private void kinectColorImage_MouseLeftButtonDown(object sender, System.Windows.Input.MouseButtonEventArgs e)
        {
            System.Windows.Point Position;
            Position = e.GetPosition(kinectColorImage);
            ColorXPersentage = Position.X / 640.0;
            ColorYPersentage = Position.Y / 480.0;
            PointColorPersentage.Text = string.Format("{0:0.00}% x {1:0.00}%", ColorXPersentage * 100, ColorYPersentage * 100);
        }

        private void sliderAngle_ValueChanged(object sender, RoutedPropertyChangedEventArgs<double> e)
        {
            if (cam != null)
                this.labelAngle.Content = this.sliderAngle.Value;
        }

        private void sliderAngle_LostMouseCapture(object sender, System.Windows.Input.MouseEventArgs e)
        {
            if (cam != null)
            {
                sliderAngle.IsEnabled = false;
                this.cam.ElevationAngle = int.Parse(this.labelAngle.Content.ToString());
                sliderAngle.IsEnabled = true;
                points = new List<Point>();
            }
        }

        bool FuckBorder(byte[] bits, int width, int height, int offset, int x, int y)
        {
            if (x < 0 || x >= width) x = -1;
            if (y < 0 || y >= height) y = -1;

            if (x == -1 || y == -1) return false;

            int j1 = Math.Max(y - offset, 0), j2 = Math.Min(y + offset, height - 1);
            int i1 = Math.Max(x - offset, 0), i2 = Math.Min(x + offset, width - 1);
            for (int i = i1; i <= i2; i++)
            {
                setColor(bits, width, i, j1, 255, 0, 0);
                setColor(bits, width, i, j2, 255, 0, 0);
            }
            for (int j = j1; j <= j2; j++)
            {
                setColor(bits, width, i1, j, 255, 0, 0);
                setColor(bits, width, i2, j, 255, 0, 0);
            }
            return true;
        }

        bool FuckPoint(byte[] bits, int width, int height, int offset, int x, int y, byte r, byte g, byte b)
        {
            if (x < 0 || x >= width) x = -1;
            if (y < 0 || y >= height) y = -1;

            if (x == -1 || y == -1) return false;
            for (int d = -offset; d <= offset; d++)
            {
                int i = x + d, j = y + d;
                if (i < 0 || i >= width) continue;
                if (j < 0 || j >= height) continue;
                setColor(bits, width, i, j, r, g, b);
            }
            for (int d = -offset; d <= offset; d++)
            {
                int i = x + d, j = y - d;
                if (i < 0 || i >= width) continue;
                if (j < 0 || j >= height) continue;
                setColor(bits, width, i, j, r, g, b);
            }
            return true;
        }

        Vector getPlane(List<Vector3> vectors)
        {
            Matrix t1 = new Matrix();
            Vector3 t2 = new Vector3();
            foreach (Vector3 v in vectors)
            {
                t1.M11 += (v.X * v.X);
                t1.M12 += (v.X * v.Y);
                t1.M13 += (v.X);
                t1.M21 += (v.X * v.Y);
                t1.M22 += (v.Y * v.Y);
                t1.M23 += (v.Y);
                t1.M31 += (v.X);
                t1.M32 += (v.Y);
                t2.X += (v.X * v.Z);
                t2.Y += (v.Y * v.Z);
                t2.Z += (v.Z);
            }
            t1.M33 = vectors.Count;
            t1.M44 = 1;
            Matrix t4 = Matrix.Invert(t1);
            Vector3 t3 = Vector3.Transform(t2, t4);
            Vector result = new Vector();
            result.X = t3.X;
            result.Y = t3.Y;
            result.Z = -1;
            result.W = t3.Z;
            return result;
        }

        void setColor(byte[] bits, int width, int x, int y, byte red, byte green, byte blue)
        {
            bits[(x + y * width) * 4 + RedIndex] = red;
            bits[(x + y * width) * 4 + GreenIndex] = green;
            bits[(x + y * width) * 4 + BlueIndex] = blue;
        }

        private void btnCalcPlane_Click(object sender, RoutedEventArgs e)
        {
            List<Vector3> points3d = new List<Vector3>();
            int width = 320, height = 240;
            int[] realDepthBak = new int[320 * 240];
            Array.Copy(realDepth, realDepthBak, 320 * 240);
            GPoint []poly = { new GPoint(LeftDownPoint), new GPoint(RightDownPoint), new GPoint(RightUpPoint), new GPoint(LeftUpPoint) };
            List<GPoint> gpoints = new List<GPoint>();
            for (int i = 0; i < depthWidth; i++)
            {
                for (int j = 0; j < depthHeight; j++)
                {
                    GPoint pt = new GPoint(i / depthWidthf, j / depthHeightf);
                    if (pt.inPoly(poly))
                    {
                        gpoints.Add(pt);
                    }
                }
            }

            double avgZ = 0;
            foreach (GPoint pt in gpoints)
            {
                int depthX = (int)Math.Round(pt.x * width);
                int depthY = (int)Math.Round(pt.y * height);
                int n = depthX + depthY * width;
                int depthValue = realDepthBak[n] << 3;
                Vector vec = Kinect.SkeletonEngine.DepthImageToSkeleton(depthX / (float)width, depthY / (float)height, (short)(depthValue));
                Vector3 vec3 = new Vector3(vec.X, vec.Y, vec.Z);
                points3d.Add(vec3);
                avgZ += vec3.Z;
            }
            Vector3[] poly3 = new Vector3[4];
            for (int i = 0; i < 4; i++)
            {
                int depthX = (int)Math.Round(poly[i].x * width);
                int depthY = (int)Math.Round(poly[i].y * height);
                int n = depthX + depthY * width;
                int depthValue = realDepthBak[n] << 3;
                Vector vec = Kinect.SkeletonEngine.DepthImageToSkeleton(depthX / (float)width, depthY / (float)height, (short)(depthValue));
                Vector3 vec3 = new Vector3(vec.X, vec.Y, vec.Z);
                poly3[i] = vec3;
            }
            avgZ /= points3d.Count;
            labelTmp.Content = points3d.Count;
//            labelTmp.Content = GPoint.areaPoly(poly);
            Vector plane = getPlane(points3d);
            double tt = 0;
            foreach (Vector3 vec3d in points3d)
            {
                tt = Math.Max(tt, Math.Abs(vec3d.X * plane.X + vec3d.Y * plane.Y + vec3d.Z * plane.Z + plane.W));
                tt /= Math.Sqrt(plane.X * plane.X + plane.Y * plane.Y + plane.Z * plane.Z);
            }
//            labelTmp.Content = avgZ;

            //            Kinect.SkeletonEngine.
            textPlane.Text = string.Format("z = \n{0}x\n{1}y\n{2}", plane.X
                , plane.Y, plane.W);

            gFileWriter.writeln(plane.X);
            gFileWriter.writeln(plane.Y);
            gFileWriter.writeln(plane.W);

            for (int i = 0; i < 4; i++)
            {
                gFileWriter.writeln(poly3[i]);
            }

            points3d.Clear();
            foreach (GPoint pt in gpoints)
            {
                int depthX = (int)Math.Round(pt.x * width);
                int depthY = (int)Math.Round(pt.y * height);
                int n = depthX + depthY * width;
                int depthValue = realDepthBak[n] << 3;
                Vector3 vec3 = new Vector3(depthX / (float)width, depthY / (float)height, depthValue);
                points3d.Add(vec3);
            }
            plane = getPlane(points3d);

            gFileWriter.writeln(plane.X);
            gFileWriter.writeln(plane.Y);
            gFileWriter.writeln(plane.W);
        }
    }
}
